package org.sp.filters;

/**
 * This is a direct port of iKalman's Matrix class with some cross reference
 * with GPSKalman.
 * 
 * @author Peter Lin
 *
 */
public class Matrix {

	private int rows;
	private int columns;
	private double data[];

	public Matrix(int rows, int columns) {
		this.rows = rows;
		this.columns = columns;
		data = new double[rows * columns];
	}

	public Matrix(Matrix value) {
		this.rows = value.rows;
		this.columns = value.columns;
		this.data = new double[this.rows * this.columns];
		for (int i = 0; i < this.data.length; ++i) {
			this.data[i] = value.data[i];
		}
	}

	public void setValue(int row, int column, double value) {
		assert row < this.rows;
		assert column < this.columns;
		this.data[row * this.columns + column] = value;
	}

	public void setAll(double... values) {
		assert values.length == this.data.length;
		for (int i = 0; i < values.length; ++i) {
			this.data[i] = values[i];
		}
	}

	public double get(int row, int col) {
		assert row < this.rows;
		assert col < this.columns;
		return this.data[row * this.columns + col];
	}

	public void setIdentityMatrix() {
		assert this.rows == this.columns;
		for (int x = 0; x < this.rows; ++x) {
			for (int i = 0; i < this.columns; ++i) {
				if (x == i) {
					setValue(x, i, 1.0);
				} else {
					setValue(x, i, 0.0);
				}
			}
		}
	}

	public String toString() {
		StringBuilder b = new StringBuilder();
		for (int x = 0; x < this.rows; ++x) {
			for (int i = 0; i < this.columns; ++i) {
				if (i > 0)
					b.append(" ");
				b.append(String.format("%.2f", get(x, i)));
			}
			b.append("\n");
		}
		return b.toString();
	}

	/**
	 * c = a + b
	 * 
	 * @p a and @p b remain unchanged.
	 */
	public static void add(Matrix a, Matrix b, Matrix c) {
		assert a.data.length == b.data.length;
		assert a.data.length == c.data.length;
		for (int i = 0; i < a.data.length; ++i) {
			c.data[i] = a.data[i] + b.data[i];
		}
	}

	/**
	 * c = a - b
	 * 
	 * @p a and @p b remain unchanged.
	 */
	public static void subtract(Matrix a, Matrix b, Matrix c) {
		assert a.data.length == b.data.length;
		assert a.data.length == c.data.length;
		for (int i = 0; i < a.data.length; ++i) {
			c.data[i] = a.data[i] - b.data[i];
		}
	}

	/**
	 * this = I - this
	 */
	public void subtractFromIdentityMatrix() {
		assert rows == columns;
		for (int i = 0; i < rows; ++i) {
			for (int j = 0; j < columns; ++j) {
				if (i == j) {
					setValue(i, j, 1.0 - get(i, j));
				} else {
					setValue(i, j, 0.0 - get(i, j));
				}
			}
		}
	}

	/**
	 * c = a * b
	 * 
	 * @p a and @p b remain unchanged.
	 */
	public static void multiply(Matrix a, Matrix b, Matrix c) {
		assert (a.columns == b.rows);
		assert (a.rows == c.rows);
		assert (b.columns == c.columns);
		for (int i = 0; i < c.rows; ++i) {
			for (int j = 0; j < c.columns; ++j) {
				// Calculate element c.data[i][j] via a dot product of one row
				// of a
				// with one column of b
				double value = 0.0;
				for (int k = 0; k < a.columns; ++k) {
					value += a.get(i, k) * b.get(k, j);
				}
				c.setValue(i, j, value);
			}
		}
	}

	/**
	 * c = a * transpose(b)
	 * 
	 * @p a and @p b remain unchanged.
	 */
	public final static void multiplyByTransposeMatrix(Matrix a, Matrix b,
			Matrix c) {
		assert (a.columns == b.columns);
		assert (a.rows == c.rows);
		assert (b.rows == c.columns);
		for (int i = 0; i < c.rows; ++i) {
			for (int j = 0; j < c.columns; ++j) {
				// Calculate element c.data[i][j] via a dot product of one row
				// of a
				// with one row of b
				double value = 0.0;
				for (int k = 0; k < a.columns; ++k) {
					value += a.get(i, k) * b.get(j, k);
				}
				c.setValue(i, j, value);
			}
		}
	}

	/**
	 * output = transpose(input)
	 */
	public final static void transpose(Matrix input, Matrix output) {
		assert (input.rows == output.columns);
		assert (input.columns == output.rows);
		for (int i = 0; i < input.rows; ++i) {
			for (int j = 0; j < input.columns; ++j) {
				output.setValue(j, i, input.get(i, j));
			}
		}
	}

	public final boolean equals(Matrix b, double tolerance) {
		assert (data.length == b.data.length);
		for (int i = 0; i < data.length; ++i) {
			if (Math.abs(data[i] - b.data[i]) > tolerance) {
				return false;
			}
		}
		return true;
	}

	/**
	 * this = this * scalar
	 */
	public final void scale(double scalar) {
		assert (scalar != 0.0);
		for (int i = 0; i < data.length; ++i) {
			data[i] *= scalar;
		}
	}

	public final void swapRows(int r1, int r2) {
		assert (r1 != r2);
		for (int col = 0; col < this.columns; ++col) {
			final double tmp = get(r1, col);
			setValue(r1, col, get(r2, col));
			setValue(r2, col, tmp);
		}
	}

	public final void scaleRow(int row, double scalar) {
		assert (scalar != 0.0);
		for (int col = 0; col < this.columns; ++col) {
			setValue(row, col, get(row, col) * scalar);
		}
	}

	/* Add scalar * row r2 to row r1. */
	public void shearRow(int row1, int row2, double scalar) {
		assert (row1 != row2);
		for (int col = 0; col < this.columns; ++col) {
			setValue(row1, col, get(row1, col) + scalar * get(row2, col));
		}
	}

	/**
	 * Uses Gauss-Jordan elimination.
	 * 
	 * The elimination procedure works by applying elementary row operations to
	 * our input matrix until the input matrix is reduced to the identity
	 * matrix. Simultaneously, we apply the same elementary row operations to a
	 * separate identity matrix to produce the inverse matrix. If this makes no
	 * sense, read wikipedia on Gauss-Jordan elimination.
	 * 
	 * This is not the fastest way to invert matrices, so this is quite possibly
	 * the bottleneck.
	 */
	public static boolean invert(Matrix input, Matrix output) {
		assert (input.rows == input.columns);
		assert (input.rows == output.rows);
		assert (input.rows == output.columns);

		output.setIdentityMatrix();

		/*
		 * Convert input to the identity matrix via elementary row operations.
		 * The ith pass through this loop turns the element at i,i to a 1 and
		 * turns all other elements in column i to a 0.
		 */
		for (int i = 0; i < input.rows; ++i) {
			if (input.get(i, i) == 0.0) {
				/* We must swap mRows to get a nonzero diagonal element. */
				int r;
				for (r = i + 1; r < input.rows; ++r) {
					if (input.get(r, i) != 0.0) {
						break;
					}
				}
				if (r == input.rows) {
					// Every remaining element in this column is zero, so this
					// matrix cannot be inverted.
					return false;
				}
				input.swapRows(i, r);
				output.swapRows(i, r);
			}

			/*
			 * Scale this row to ensure a 1 along the diagonal. We might need to
			 * worry about overflow from a huge scalar here.
			 */
			double scalar = 1.0 / input.get(i, i);
			input.scaleRow(i, scalar);
			output.scaleRow(i, scalar);

			/* Zero out the other elements in this column. */
			for (int j = 0; j < input.rows; ++j) {
				if (i == j) {
					continue;
				}
				double shear_needed = -input.get(j, i);
				input.shearRow(j, i, shear_needed);
				output.shearRow(j, i, shear_needed);
			}
		}
		return true;
	}
}
